package es.esa.integral;

/**
 * Created by esalazar on 4/28/14.
 */
/**
 * Perform operations on a 3x3 matrix
 * @author Nigel Dean & Frans Jacobs
 */
public class Matrix {

    protected double[][] m_matrix;
    protected double[][] m_inverse;
    protected double m_determinant;

    /**
     * @return a 3x3 matrix with the current data
     */
    public double[][] getMatrix() {
        return m_matrix;
    }

    /**
     * @return the determinant.
     */
    public double getDeterminant() {
        return m_determinant;
    }

    /**
     * @return the inverse matrix.
     */
    public double[][] getInverse() {
        return m_inverse;
    }

    /**
     * @param inputData One dimensional array containing matrix data.
     * @throws IllegalArgumentException when the input does not have 9 elements
     */
    public Matrix(double[] inputData) {
        if (inputData.length != 9)
            throw new IllegalArgumentException("Matrix constructor called with invalid number of elements");

        m_matrix = new double[3][3];

        for (int i = 0; i < 9; i++) {
            m_matrix[i/3][i%3] = inputData[i];
        }

        generateDeterminant();
        generateInverse();
    }

    /**
     * @param r1c1 data for row 1 column 1
     * @param r1c2 data for row 1 column 2
     * @param r1c3 data for row 1 column 3
     * @param r2c1 data for row 2 column 1
     * @param r2c2 data for row 2 column 2
     * @param r2c3 data for row 2 column 3
     * @param r3c1 data for row 3 column 1
     * @param r3c2 data for row 3 column 2
     * @param r3c3 data for row 3 column 3
     */
    public Matrix(double r1c1, double r1c2, double r1c3,
                  double r2c1, double r2c2, double r2c3,
                  double r3c1, double r3c2, double r3c3) {

        m_matrix = new double[3][3];
        m_matrix[0][0] = r1c1;
        m_matrix[0][1] = r1c2;
        m_matrix[0][2] = r1c3;
        m_matrix[1][0] = r2c1;
        m_matrix[1][1] = r2c2;
        m_matrix[1][2] = r2c3;
        m_matrix[2][0] = r3c1;
        m_matrix[2][1] = r3c2;
        m_matrix[2][2] = r3c3;

        generateDeterminant();
        generateInverse();
    }


    /**
     * @param inputData Two dimensional array containing matrix data.
     * @throws IllegalArgumentException when the input is not a 3x3 matrix
     */
    public Matrix(double[][] inputData) {
        m_matrix = new double[3][3];

        try {
            for (int i = 0; i < 3; i++) {
                for (int j = 0; j < 3; j++) {
                    m_matrix[i][j] = inputData[i][j];
                }
            }
        } catch (ArrayIndexOutOfBoundsException a) {
            throw new IllegalArgumentException("ISOCMatrix constructor called with invalid number of elements");
        }

        generateDeterminant();
        generateInverse();
    }

    /**
     * Generates the determinant of the matrix.
     */
    private void generateDeterminant() {
        m_determinant = m_matrix[0][0]*m_matrix[1][1]*m_matrix[2][2] -
                m_matrix[0][0]*m_matrix[1][2]*m_matrix[2][1] +
                m_matrix[0][1]*m_matrix[1][2]*m_matrix[2][0] -
                m_matrix[0][1]*m_matrix[1][0]*m_matrix[2][2] +
                m_matrix[0][2]*m_matrix[1][0]*m_matrix[2][1] -
                m_matrix[0][2]*m_matrix[1][1]*m_matrix[2][0];
    }

    /**
     * Generates and inverse matrix.
     */
    private void generateInverse() {
        m_inverse = new double[3][3];

        m_inverse[0][0] = m_matrix[1][1] * m_matrix[2][2] - m_matrix[1][2]*m_matrix[2][1];
        m_inverse[1][0] = -(m_matrix[1][0] * m_matrix[2][2] - m_matrix[1][2]*m_matrix[2][0]);
        m_inverse[2][0] = m_matrix[1][0] * m_matrix[2][1] - m_matrix[1][1]*m_matrix[2][0];
        m_inverse[0][1] = -(m_matrix[0][1] * m_matrix[2][2] - m_matrix[0][2]*m_matrix[2][1]);
        m_inverse[1][1] = m_matrix[0][0] * m_matrix[2][2] - m_matrix[0][2]*m_matrix[2][0];
        m_inverse[2][1] = -(m_matrix[0][0] * m_matrix[2][1] - m_matrix[0][1]*m_matrix[2][0]);
        m_inverse[0][2] = m_matrix[1][2] * m_matrix[0][1] - m_matrix[1][1]*m_matrix[0][2];
        m_inverse[1][2] = -(m_matrix[0][0] * m_matrix[1][2] - m_matrix[0][2]*m_matrix[1][0]);
        m_inverse[2][2] = m_matrix[1][1] * m_matrix[0][0] - m_matrix[1][0]*m_matrix[0][1];
    }

    /**
     * Performs a matrix transformation on a 3-element vector
     * @param inVector 3-element vector
     * @throws IllegalArgumentException when the inVector does not have 3 elements
     */
    public double[] transform(double[] inVector) {
        if (inVector.length != 3) {
            throw new IllegalArgumentException("Invalid number of elements for transform");
        }

        double[] outVector = {0.0, 0.0, 0.0};
        for (int i = 0; i < 3; i++) {
            for (int j = 0; j < 3; j++) {
                //Changed order of rows and columns
                outVector[i] = outVector[i] + m_matrix[j][i] * inVector[j];
            }
        }
        return outVector;
    }

    /**
     * Performs the inverse matrix transformation on a 3-element vector
     * @param  inVector 3-element vector
     * @throws IllegalArgumentException when the inVector does not have 3 elements
     */
    public double[] inverseTransform(double[] inVector) {
        if (inVector.length != 3) {
            throw new IllegalArgumentException("Invalid number of elements for transform");
        }

        double[] outVector = {0.0, 0.0, 0.0};
        for (int i = 0; i < 3; i++) {
            for (int j = 0; j < 3; j++) {
                outVector[i] = outVector[i] + m_inverse[j][i] * inVector[j];
            }
            outVector[i] = outVector[i] / m_determinant;
        }
        return outVector;
    }

    /**
     * Performs a matrix transformation on another 3x3 matrix
     * @param inMatrix an initialized Matrix
     */
    public Matrix transform(Matrix inMatrix) {
        double[][] result = new double[3][3];
        double[][] inMx = inMatrix.getMatrix();

        for (int i = 0; i < 3; i++) {
            for (int j = 0; j < 3; j++) {
                result[i][j] = 0.0;
                for (int k = 0; k < 3; k++) {
                    result[i][j] = result[i][j] + m_matrix[i][k] * inMx[k][j];
                }
            }
        }
        return new Matrix(result);
    }

    /**
     * Performs the inverse matrix transformation on another 3x3 matrix
     * @param inMatrix an initialized Matrix
     */
    public Matrix inverseTransform(Matrix inMatrix) {
        double[][] result = new double[3][3];
        double[][] inMx = inMatrix.getMatrix();

        for (int i = 0; i < 3; i++) {
            for (int j = 0; j < 3; j++) {
                result[i][j] = 0.0;
                for (int k = 0; k < 3; k++) {
                    result[i][j] = result[i][j] + m_inverse[i][k] * inMx[k][j];
                }
                result[i][j] = result[i][j] / m_determinant;
            }
        }
        return (new Matrix(result));
    }

    /**
     * @param a a 3 element vector
     * @param b a 3 element vector
     * @return the cross-product of two 3-element vectors.
     */
    public static double[] getCrossProduct(double[] a, double[] b) {
        double[] xProduct = new double[3];
        xProduct[0] = a[1] * b[2] - a[2] * b[1];
        xProduct[1] = a[2] * b[0] - a[0] * b[2];
        xProduct[2] = a[0] * b[1] - a[1] * b[0];
        return xProduct;
    }


    /**
     * @param rar [radians]
     * @param decr [radians].
     * @return a unit vector in the intertial-XYZ frame of reference
     *  for the direction specified by the source Position
     */
    public static double[] getUnitVector(double rar, double decr) {
        double[] unitVector = new double[3];
        unitVector[0] = Math.cos(rar) * Math.cos(decr);
        unitVector[1] = Math.sin(rar) * Math.cos(decr);
        unitVector[2] = Math.sin(decr);

        return unitVector;
    }

    /**
     * Transpose the Matrix data
     * @param mData matrix data in the order r1c1,r1c2,r1c3,r2c1,r2c2,r2c3,r3c1,r3c2,r3c3
     * @return the transpose order (r1c1,r2c1,r2c1,r1c3,r2c2,r3c2,r1c3,r2c3,r3c3)
     */
    public static double[] transpose(double[] mData) {
        double[] transpose = new double[mData.length];
        transpose[0] = mData[0];
        transpose[1] = mData[3];
        transpose[2] = mData[6];
        transpose[3] = mData[1];
        transpose[4] = mData[4];
        transpose[5] = mData[7];
        transpose[6] = mData[2];
        transpose[7] = mData[5];
        transpose[8] = mData[8];
        return transpose;
    }

    /**
     * @return the matrix data is a single array in the order r1c1,r1c2,r1c3,r2c1,r2c2....
     */
    public double[] getData() {
        double[] d = new double[9];
        int idx = 0;
        for(int r=0;r<3;r++) {
            for(int c=0;c<3;c++) {
                d[idx] = m_matrix[r][c];
                idx++;
            }
        }
        return d;
    }

    /**
     * Transpose the Matrix
     * @param m a Matrix to transpose
     * @return the transpose Matrix of m)
     */
    public static Matrix transpose(Matrix m) {
        double[] mData = m.getData();
        double[] tData = transpose(mData);
        return new Matrix(tData);
    }

    /**
     * @return the contenst of the matrix as a String
     */
    public String toString() {
        StringBuffer b = new StringBuffer();
        for (int r = 0; r < 3; r++) {
            for (int c = 0; c < 3; c++) {
                b.append(m_matrix[r][c]);
                b.append(", ");
            }
            b.append("\n");
        }
        //remove the last "\n"
        int idx = b.lastIndexOf("\n");
        b.deleteCharAt(idx);
        //remove the last " "
        idx = b.lastIndexOf(" ");
        b.deleteCharAt(idx);
        //remove the last ","
        idx = b.lastIndexOf(",");
        b.deleteCharAt(idx);
        return b.toString();
    }
}
